# 1.导包；
import rclpy
from rclpy.node import Node
from std_msgs.msg import String #topic1
from std_msgs.msg import Float32 #topic1
from std_msgs.msg import Header
from geometry_msgs.msg import PoseWithCovariance #odom的pose的格式
from geometry_msgs.msg import PoseStamped #ap/pose的格式
from geometry_msgs.msg import TwistWithCovariance #odom的twist的格式 
from geometry_msgs.msg import TwistStamped #ap/twist的格式
from sensor_msgs.msg import Imu #ap/imu的数据格式，和ROS的imu格式相同
from nav_msgs.msg import Odometry #ROS的odom的格式
from rclpy.qos import QoSProfile, ReliabilityPolicy # QOS
from sensor_msgs.msg import Imu  # 假设原始话题类型为Imu消息类型

class TopicRename(Node):
    def __init__(self):
        super().__init__('Imu_rename')#节点名称

        # 设置为不可靠（BEST_EFFORT）的QoS策略
        qos_profile = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )
        # 订阅原始话题
        self.subscription = self.create_subscription(
            Imu,
            '/ap/imu/experimental/data',  # 原始话题
            self.listener_callback,
            qos_profile
        )

        # 发布到新的话题
        self.publisher_imu = self.create_publisher(Imu, '/imu', 10)
        self.get_logger().info("等待imu...") 

    def listener_callback(self, msg_imu):
        # 将收到的消息发布到新话题
        self.publisher_imu.publish(msg_imu)
        
     

def main(args=None):
    rclpy.init(args=args)

    imu_rename = TopicRename()

    rclpy.spin(imu_rename)

    # 关闭节点
    imu_rename.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
